using System;
using System.Collections.Generic;
using System.Linq;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using Microsoft.Kinect;
using System.Diagnostics; 


namespace KinectVDRTest
{
    public class Game1 : Microsoft.Xna.Framework.Game
    {
        #region Fields

        readonly GraphicsDeviceManager graphics;
        Vector3 modelPosition = Vector3.Zero;
        float modelRotation = 0.0f;
        Vector3 cameraPosition = new Vector3(0.0f, 0.0f, 200.0f);
        KinectSensor kinectSensor;
        SpriteBatch spriteBatch;
        Texture2D colorTexture;
        Model myModel;
        float aspectRatio;
        byte[] bits;
        private Skeleton[] skeletons = null;
        float pose;
        Matrix handMatrix;
        Vector3 point;
        private byte[] colorBuffer;
        BoneOrientation rotation;
        bool kinectNotPowered = true;
        bool notPowerd;
        bool connected;
        String message="PLEASE MAKE SURE THE KINECT IS PLUGED IN AND TURENED ON";

        #endregion

        #region Properties

        public bool IsRunning
        {
            get { return kinectSensor != null; }
            
        }

        SpriteFont SpriteFont { get; set; }

        #endregion

        #region Kinect Launch 
        void LaunchKinect()
        {
            try
            {
                kinectSensor = KinectSensor.KinectSensors[0];
                kinectSensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                kinectSensor.SkeletonStream.Enable(new TransformSmoothParameters()
                {
                    Smoothing = 0.5f,
                    Correction = 0.5f,
                    Prediction = 0.5f,
                    JitterRadius = 0.05f,
                    MaxDeviationRadius = 0.04f
                });
                kinectSensor.Start();

                kinectSensor.SkeletonFrameReady += SensorSkeletonFrameReady;
                kinectSensor.SkeletonStream.Enable();
                kinectNotPowered = false;
                connected = true;
            }
            catch
            {
                return;
            }
        }

        private void KinectSensorsStatusChanged(object sender, StatusChangedEventArgs e)
        {
            var status = e.Status;
            if (this.kinectSensor == null)
            {
                this.UpdateStatus(status);
                if (e.Status == KinectStatus.Connected)
                {
                    this.kinectSensor = e.Sensor;
                }
            }
            else
            {
                if (this.kinectSensor == e.Sensor)
                {
                    this.UpdateStatus(status);
                    if (e.Status == KinectStatus.Disconnected ||
                        e.Status == KinectStatus.NotPowered)
                    {
                        this.kinectSensor = null;
                    }
                }
            }
        }

        private void UpdateStatus(KinectStatus status)
        {
            switch(status)
            {
                case KinectStatus.Disconnected:
                    kinectNotPowered = false;
                    message = "COULD NOT INITIALIZE KINECT DEVICE! \n PLEASE MAKE SURE THE DEVICE IS CONNECTED.";
                    break;

                case KinectStatus.NotPowered:
                    kinectNotPowered = false;
                    message = "THE DEVICE IS PLUGED IN PLEASE MAKE SURE IT IS TURNED ON.";
                    break;

                case KinectStatus.Initializing:
                    message = "Initializing...";
                    break;                    

                case KinectStatus.Connected:
                    LaunchKinect();
                    kinectNotPowered = false;
                    connected = true;
                    break;
            }
        }

        void GetVideoFrame()
        {
            if (kinectSensor == null)
                return;
            if (kinectNotPowered)
            {
                return;
            }
           
            var frame = kinectSensor.ColorStream.OpenNextFrame(0);
            
            
            if (frame == null)
                return;

            if (bits == null || bits.Length != frame.PixelDataLength)
            {
                bits = new byte[frame.PixelDataLength];
            }
            frame.CopyPixelDataTo(bits);
            UpdateColorTexture(bits);
            // Reset textures used to null in order to update it
            GraphicsDevice.Textures[0] = null;
            GraphicsDevice.Textures[1] = null;
            GraphicsDevice.Textures[2] = null;
            // Update the texture
            colorTexture.SetData(colorBuffer);
        }

        public void UpdateColorTexture(byte[] bits)
        {
            if (colorBuffer == null)
                colorBuffer = new byte[640 * 480 * 4];
            for (int index = 0; index < colorTexture.Width * colorTexture.Height; index++)
            {
                byte b = bits[index * 4];
                byte g = bits[index * 4 + 1];
                byte r = bits[index * 4 + 2];
                colorBuffer[index * 4] = r;
                colorBuffer[index * 4 + 1] = g;
                colorBuffer[index * 4 + 2] = b;
                colorBuffer[index * 4 + 3] = 0;
            }
        }

        protected override void OnExiting(Object sender, EventArgs args)
        {
            base.OnExiting(sender, args);
            StopKinect(kinectSensor);
        }

        private void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    sensor.Stop();

                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }
                }
            }
        }
        #endregion

        public Game1()
        {
            graphics = new GraphicsDeviceManager(this)
            {
                PreferredBackBufferWidth = 800,
                PreferredBackBufferHeight = 600,
                GraphicsProfile = GraphicsProfile.HiDef,
            };
            IsFixedTimeStep = true;
            TargetElapsedTime = TimeSpan.FromMilliseconds(30);
            Content.RootDirectory = "Content";
            LaunchKinect();
            KinectSensor.KinectSensors.StatusChanged += this.KinectSensorsStatusChanged;           
        }

        protected override void Update(GameTime gameTime)
        {
            if (GraphicsDevice == null)
                return;            
            modelRotation = pose*3;          
            GetVideoFrame();
            base.Update(gameTime);
        }       

        protected override void Draw(GameTime gameTime)
        {
            GraphicsDevice.Clear(Color.Black);
            GraphicsDevice.BlendState = BlendState.Opaque;
            spriteBatch.Begin();
            spriteBatch.Draw(colorTexture, new Rectangle(0, 0, GraphicsDevice.Viewport.Width, GraphicsDevice.Viewport.Height), Color.White);
            spriteBatch.End();                        
            Matrix[] transforms = new Matrix[myModel.Bones.Count];
            myModel.CopyAbsoluteBoneTransformsTo(transforms);
            float scale = 1f;

            if (connected)
            {
                spriteBatch.Begin();
            // Draw the model. A model can have multiple meshes, so loop.
            foreach (ModelMesh mesh in myModel.Meshes)
            {
                // This is where the mesh orientation is set, as well 
                // as our camera and projection.
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.EnableDefaultLighting();

                    //effect.World = transforms[mesh.ParentBone.Index] *
                    //    Matrix.CreateRotationY(modelRotation)
                    //    * Matrix.CreateTranslation(point) * handMatrix;
                   // effect.World = Matrix.CreateScale(1f, 1f, 1f) * handMatrix * Matrix.CreateTranslation(point * new Vector3(1, 1, -1f));
                   // effect.World = Matrix.CreateRotationX(-1.571428571428571f) * Matrix.CreateRotationY(modelRotation) * Matrix.CreateTranslation(point) * Matrix.CreateScale(scale);
                    
                    effect.World = Matrix.CreateRotationX(-1.571428571428571f) * Matrix.CreateRotationY(modelRotation) * Matrix.CreateTranslation(point) * Matrix.CreateScale(scale);
                    effect.View = Matrix.CreateLookAt(cameraPosition,
                        Vector3.Zero, Vector3.Up);

                    effect.Projection = Matrix.CreatePerspectiveFieldOfView(
                        MathHelper.ToRadians(45.0f), aspectRatio,
                        1.0f, 10000.0f);
                }
                
                mesh.Draw();
            }

            }
            else
            {
                spriteBatch.Begin();
                // Draw display message.      
                float left = MathHelper.Max(GraphicsDevice.Viewport.TitleSafeArea.Left, 20);
                float top = MathHelper.Max(GraphicsDevice.Viewport.TitleSafeArea.Top, 20);
                Vector2 position = new Vector2(left, top);
                position = new Vector2(170, 250);
                spriteBatch.DrawString(SpriteFont, message, position, Color.Red);
            }

            spriteBatch.End();
            base.Draw(gameTime);
        }
        
               

        protected override void LoadContent()
        {
            spriteBatch = new SpriteBatch(GraphicsDevice);
            colorTexture = new Texture2D(GraphicsDevice, 640, 480, false, SurfaceFormat.Color);
            myModel = Content.Load<Model>("Models\\box");
            SpriteFont = Content.Load<SpriteFont>("SpriteFont1");
            aspectRatio = graphics.GraphicsDevice.Viewport.AspectRatio;
        }                

        private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    if (this.skeletons == null)
                    {
                        // Allocate array of skeletons
                        this.skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    }

                    // Copy skeletons from this frame
                    skeletonFrame.CopySkeletonDataTo(this.skeletons);

                    // Find first tracked skeleton, if any
                    Skeleton skeleton = this.skeletons.Where(s => s.TrackingState == SkeletonTrackingState.Tracked).FirstOrDefault();

                    if (skeleton != null)
                    { 
                        //rotation = skeleton.BoneOrientations[JointType.HipRight].AbsoluteRotation;
                        Joint j = skeleton.Joints[JointType.HipCenter];
                        Joint sholderR = skeleton.Joints[JointType.ShoulderRight];
                        Joint sholderL = skeleton.Joints[JointType.ShoulderLeft];
                        pose = sholderR.Position.Z - sholderL.Position.Z;
                        point.X = j.Position.X * 2000 ;
                        point.Y = (j.Position.Y-0.15f) * 2000;
                        point.Z = -j.Position.Z * 2000;
                        
                        handMatrix = skeleton.BoneOrientations.Where(
                        b => b.EndJoint == JointType.HandLeft)
                        .Select(b => b.AbsoluteRotation).FirstOrDefault().Matrix.ToMatrix();
                     
                        if (j.TrackingState == JointTrackingState.Tracked)
                        {
                            //Console.WriteLine("Left knee: " + j.Position.X + ", " + j.Position.Y + ", " + j.Position.Z);
                        }
                    }
                }
            }            
        }        
    }
}